#include <cstdio>
#include <iostream>
#include "scorbot.h"
#include "vision.h"
#include "string.h"

using namespace std;

#define DEBUG 1

poDesc_t *scan(IMAGE_TARGET target) {
	int pos_length = 10;
	poDesc_t pos[] = { 
					   { -500, 4500, 3500, -900, 0, 0},
					   { 1000, 4000, 3500, -900, 0, 0},
					   { 2500, 3500, 3500, -900, 0, 0},
					   { 3500, 2500, 3500, -900, 0, 0},
					   { 4100, 1300, 3500, -900, 0, 0},
					   { 4200, -250, 3500, -900, 0, 0},
					   { 3900, -1800, 3500, -900, 0, 0},
					   { 3100, -2900, 3500, -900, 0, 0},
					   { 2400, -3500, 3500, -900, 0, 0},
					   { 1500, -4000, 3500, -900, 0, 0},
					  };
	H_Matrix mat_b2g, mat_g2b;
	poDesc_t *move, poFG;
	int idx = 0;

	while(1) {
		if (scorbot_MoveAbs(&pos[idx], (idx == 0) ? 4 : 1) != 0) {
			cout << "Error moving.. die" << endl;
			return NULL;
		}

		poFG = makePoFromScorbotXYZPR(pos[idx].x, pos[idx].y, pos[idx].z, pos[idx].phi, pos[idx].rho);
		if(DEBUG == 1){
			cout	<< "position from gregs stuff: x=" << poFG.x << " y=" << poFG.y << " z=" << poFG.z
					<< " rho=" << poFG.rho << " phi=" << poFG.phi << " theta=" << poFG.theta << endl;
		}

		// Check for block
		makeHB2GFromPo(poFG, &mat_b2g);
		makeHG2BFromHB2G(&mat_b2g, &mat_g2b);

		if ( (move = getBlockCoordinates(&mat_g2b, target)) != NULL) {
			if(DEBUG == 1)
				cout << target <<" DETECTED!!!" << endl;

			cout << move->x << " " << move->y << " " << move->z << " " << endl;
			return move;
		}

		idx++;
		if (idx >= pos_length) {
			// Start over from first position
			idx = 0;
			setSpeed(80);
			sleep(5); // sleep for 5 seconds
		} else {
			setSpeed(15);
		}
	}
}

int main() {

	poDesc_t *position = NULL;

	// Initialize serial and scan for block
	if(driver_init() != 0)	{
		cout << "Problems initializing\n";
		return 1;
	}

	// Initialize vision stuff
	if (camera_setup() != 0) {
		cout << "Problems setting up camera" << endl;
		return 1;
	}


	cout << "Press any key to begin scanning" << endl;
	getchar();	

	setSpeed(80);
	while(1)	{
		if((position = scan(BLOCK)) == NULL)	{
			cout << "What?!?" << endl;
			return -1;
		}

		//open grippers
		if(open_gripper() != 0)	{
			cout << "Problem opening grippers!\n";
			continue;
		}

		setSpeed(50);
		if(scorbot_MoveAbs(position, 2) != 0)	{
			cout << "PROBLEMS WITH MOVING DOWN!\n";
			break;
		}

		//block should be within gripper after moving
		//close gripper
		if(close_gripper() != 0) {
			cout << "Problem closing grippers!\n";
			continue;
		}

		// scan for landing zone
		if((position = scan(AREA)) == NULL)	{
			cout << "WHAT?!" << endl;
			return -1;
		}

		position->z += 60;
		setSpeed(15);
		if(scorbot_MoveAbs(position, 4) != 0)	{
			cout << "PROBLEMS WITH MOVING DOWN!\n";
			break;
		}

		//open grippers
		if(open_gripper() != 0)	{
			cout << "Problem opening grippers!\n";
			continue;
		}

		setSpeed(50);
		scorbot_MoveC("move 1", 2);
		cout << "Click enter to continue looping to scan for another block:\n";
		getchar();
	}
	



	commDriver_Uninit();
}
